from xgolib import XGO
import time
from ultralytics import YOLO
import cv2
import os
from detect import Detector
from DogControl import DogController
from xgoedu import XGOEDU
import datetime
import inspect
dog_edu=XGOEDU()
det = Detector(dog_edu)
dog_control = DogController(dog_edu)

def logfile(content):
    # 获取当前时间（格式：YYYY-MM-DD HH:MM:SS）
    current_time = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S")
    # 获取调用 self.loginfo 的文件名和行号
    frame = inspect.currentframe().f_back  # 获取调用栈的上一帧
    filename = os.path.basename(frame.f_code.co_filename)  # 提取文件名（不含路径）
    line_no = frame.f_lineno  # 行号
    # 格式化输出：[日期]-[文件名]-[行号]: 内容
    log_message = f"[{current_time}]-[{filename}]-[{line_no}]: {content}"
    with open("log.txt","a",encoding="utf-8") as f:
        log_message = log_message + "\n"
        f.write(log_message)
def Move(direct=None,t=None):
    if direct == None:
        direct = input("请输入运动方向，后和右在后面输入: （前后左右）")
    if t == None:
        t = float(input("请输入运动时间:"))
    logfile(f"此时运动的方向为:{direct},运动时间为:{t}")
    dog_control.move(direct,t)
    
def Turn(angle=None,t=None):
    if angle == None:
        angle = int(input("请输入旋转角度:90(逆时针),-90(顺时针)"))
    if t == None:
        t = float(input("请输入转身时间:"))
    logfile(f"此时运动的角度为:{angle},运动时间为:{t}")
    dog_control.turn(angle,t)
def ShapeDetect():
    input("请输入enter键开始下蹲:")
    dog_control.Crouch()
    result = det.detect_once_shape_position()
    if len(result['detected_objects']) > 0:
        logfile(f"中心点坐标:{result['detected_objects'][0]['center_x']}")    
        logfile(f"宽度:{result['detected_objects'][0]['width']}")    
    else:
        logfile("识别失败")
    dog_control.Reset()
def auto_shape_detect():
    dog_control.Crouch()
    result = det.auto_detect_once_shape_position()
    ret =  []
    if len(result['detected_objects']) > 0:
        center_x = result['detected_objects'][0]['center_x']
        width = result['detected_objects'][0]['width']
        saved_file = result['saved_file']
        logfile(f"当前中心点坐标:{center_x},当前宽度：{width}")
        ret.append(center_x)
        ret.append(width)
        ret.append(saved_file)
    else:
        dog_control.Speech("识别失败")
    dog_control.Reset()   
    return ret 
def DangerDetect():
    result = det.auto_shape_detect()
    if len(result) > 0:
        logfile(result[2])
    else:
        logfile("识别失败")
def CathchOrPut():
    op = input("输入c表示抓取,p表示放置:")
    if op == 'c':
        Cathch()
    else:
        PutOn()
def Cathch():
    dog_control.catch_from_ground()
def PutOn():
    dog_control.put_on_cylinder()
def auto_locate_cylinder():
    flg=False  # 左右，前后方向是否调整完毕
    X,H = None,None
    for i in range(2):
        ok1,ok2 = False,False
        offset,stand_x = 4,165  # 距离目标中心点的偏差,目标的中心点坐标
        last_offset,last_direct,last_time = None,None,None # 前一次的偏移量,前一次运动的方向
        first_time = 1 # 默认初始的偏移时间
        if flg:
            first_time = 0.8 
        while not ok1:
            logfile("#########################开始调整左右#########################")
            ret = auto_shape_detect()
            if len(ret) > 0:
                center_x = ret[0]
                logfile(f"保存文件路径:{ret[2]}")
            else:
                dog_control.Speech("识别失败，退出调节左右")
                break 
            cur_offset = abs(center_x-stand_x)
            logfile(f"当前偏移量:{cur_offset},上一次偏移量:{last_offset}")
            if cur_offset <= offset:
                ok1 = True
                logfile("#########################左右调整完毕#########################")
                break 
            if last_offset == None: # 第一次的话,直接根据目标中心点直接便宜
                if center_x < stand_x:
                    Move('左',first_time)
                    last_direct = '左'
                else:
                    Move('右',first_time)
                    last_direct = '右'
                logfile(f"说明此时圆台在我们的左边，我们要向左移动，当前坐标:{center_x},\
                        上一次的偏移量:{last_offset},上一次的运动方向：{last_direct},上一次的运动时间:{last_time}\
                        当前运动的时间:{first_time}")            
                last_time = first_time 
                last_offset = cur_offset # 并把此时的偏移量保存下来 
            # 接下来就是根据历史的方向和移动时间来计算当前合适的运动时间，但是注意时间最好不要低于0.6
            # 因为有可能狗是斜着的，所以我们以上一次的运动方向来判断
                continue 
            cur_time = 0.6
            if center_x < stand_x: # 直接向左移动即可
                tmp_time = last_time*(cur_offset/last_offset)
                if cur_time < tmp_time and tmp_time <= 0.8:
                    cur_time = tmp_time
                logfile(f"此时圆台在我们的左边，所以向左移动，当前坐标:{center_x},\
                                    上一次的偏移量:{last_offset},上一次的运动方向：{last_direct},上一次的运动时间:{last_time}\
                                        当前运动的时间:{cur_time}")    
                last_time = cur_time
                last_offset = cur_offset
                last_direct =  '左'
                Move(last_direct,cur_time)  
                continue           
            else: 
                tmp_time = last_time*(cur_offset/last_offset)
                if cur_time < tmp_time and tmp_time <= 0.8:
                    cur_time = tmp_time
                logfile(f"此时圆台在我们的右边,所以向右运动，当前坐标:{center_x},\
                                    上一次的偏移量:{last_offset},上一次的运动方向：{last_direct},上一次的运动时间:{last_time}\
                                        当前运动的时间:{cur_time}")    
                last_time = cur_time
                last_offset = cur_offset
                last_direct =  '右'
                Move(last_direct,cur_time)
                continue 
        w_offset,w_stand = 3,125
        w_first_time = 1.2
        w_last_offset,w_last_time,w_last_direct = None,None,None 
        if flg:
            w_first_time = 0.8
        while not ok2:
            logfile("#########################开始调整前后方向#########################")
            ret = auto_shape_detect()
            if len(ret) > 0:
                X = ret[0]
                H = ret[1]
                cur_width = ret[1]
                logfile(f"保存文件路径:{ret[2]}")
            else:
                dog_control.Speech("识别失败，退出调节前后")
                if ok1 and not flg:
                    Move('后',0.8)
                    flg = True
                    continue 
                else:
                    break 
            w_cur_offset = abs(cur_width-w_stand)
            if w_cur_offset <= w_offset:
                logfile("#########################前后调整完毕#########################")
                ok2 = True
                break 
            if w_last_offset == None: # 说明第一次，直接走
                w_last_direct = '前'
                w_last_offset = w_cur_offset
                w_last_time = w_first_time
                Move('前',w_first_time)
                continue
            w_cur_time = 0.6 
            if cur_width < w_stand:
                w_tmp_time = w_last_time*(w_cur_offset/w_last_offset)
                if w_tmp_time > w_cur_time and w_tmp_time <= 0.8:
                    w_cur_time = w_tmp_time
                logfile(f"此时宽度小于目标宽度,所以向前移动，当前宽度:{cur_width},\
                                    上一次的偏移量:{w_last_offset},上一次的运动方向：{w_last_direct},上一次的运动时间:{w_last_time}\
                                        当前运动的时间:{w_cur_time}")                    
                w_last_time = w_cur_time
                w_last_offset = w_cur_offset
                w_last_direct = '前'
                Move('前',w_cur_time)
                continue
            else : 
                w_tmp_time = w_last_time*(w_cur_offset/w_last_offset)
                if w_tmp_time > w_cur_time and w_tmp_time <= 0.8:
                    w_cur_time = w_tmp_time
                logfile(f"此时宽度大于目标宽度，所以向后移动一点,当前宽度:{cur_width},\
                                    上一次的偏移量:{w_last_offset},上一次的运动方向：{w_last_direct},上一次的运动时间:{w_last_time}\
                                        当前运动的时间:{w_cur_time}")                     
                w_last_time = w_cur_time
                w_last_offset = w_cur_offset
                w_last_direct = '后'
                Move('后',w_cur_time)
                continue
        logfile("位置调整完毕")
        if (X >= 165 and X <= 169) and (H >= 122 and H <= 128):
            break 
def main():
    while True:
        op = int(input('''\n
        1. 进行移动(上下左右)\n
        2. 进行转身\n
        3. 进行抓,放\n
        4 识别形状,得出结论   
        5. 危险标志识别       
        '''))
        if op == 1:
            Move() 
        elif op == 2:
            Turn() 
        elif op == 3:
            CathchOrPut()
        elif op == 4:
            ShapeDetect()
        elif op == 5:
            DangerDetect()             
        else:
            logfile("输出错误，范围1~5，请重新输入!!")
            continue 
# pos: "方向 时间"
array = [["1 左 1.5","1 前 1.5"],["5"],["1 左 4.78","1 前 3.2"],["5"],["1 右 2.2","1 前 3","1 右 3","1 前 4","2 90 1.5","1 左 1","1 前 8","1 左 1.5"]]
if __name__ == "__main__":
    c = input("是否开启自动巡航过程?")
    if c == "yes":
        for pos in array:
            # 每一个位置
            for i in pos:
                positions = i.split(" ")
                op = int(positions[0])
                logfile(f"当前执行操作：{op}")
                if op == 1:
                    direct,t = positions[1],float(positions[2])
                    Move(direct,t) 
                elif op == 2:
                    angle,t = int(positions[1]),float(positions[2])
                    Turn(angle,t) 
                    dog_control.Reset()
                    time.sleep(1)
                elif op == 3:
                    CathchOrPut()
                elif op == 4:
                    ShapeDetect()
                    time.sleep(1.2)
                elif op == 5:
                    DangerDetect() 
                    time.sleep(1.2)
                else:
                    logfile("输出错误，范围1~5，请重新输入!!")
    # auto_locate_cylinder()            
    # auto_locate_cylinder()        
    main()         